#include "directsampling.h"
#include <cmath>

float direct_sampling_cont(float *ti, int length, int width, int height,
                           float *dv, int NR_try, float tolerance,
                           dv_bnd & bnd)
{
    int xsize = length + bnd.dvminx - bnd.dvmaxx;
    int ysize = width + bnd.dvminy - bnd.dvmaxy;
    int zsize = height + bnd.dvminz - bnd.dvmaxz;
    float closest_value = 1024;
    float closest_diff = 1024;
    double dmax = 255;
    for (int i = 0; i < NR_try; i++)
    {
        int xpos = rand() % xsize - bnd.dvminx;
        int ypos = rand() % ysize - bnd.dvminy;
        int zpos = rand() % zsize - bnd.dvminz;
        double diff = 0;
        for (int dvi = 0; dvi < dv[0]; dvi++)
        {
            diff +=
                fabs(ti
                     [xpos + int (dv[1 + dvi * 4 + 0]) +
                      length * (ypos +
                                int (dv[1 + dvi * 4 + 1])) +length *
                      width * (zpos + int (dv[1 + dvi * 4 + 2]))] - dv[1 +
                                                                       4 *
                                                                       dvi
                                                                       +
                                                                       3]);
        }
        if (dv[0] > 0)
            diff = diff / dmax / dv[0];
        if (diff <= closest_diff)
        {
            closest_value =
                ti[xpos + length * ypos + length * width * zpos];
            closest_diff = diff;
        }
        if (diff < tolerance)
            break;
    }
    return closest_value;
}

void Simulation_cont(Index < L2 < float > >*R, float *ti, int NR_neighbor,
                     int NR_try, float tolerance, vector < coord > &sample,
                     vector < int >&ids, float *target, int N_sample,
                     int tilength, int tiwidth, int tiheight, int unitsize,
                     int start_id, int level, int length, int width,
                     int height)
{
    //The path is already random.
    float *dv = new float[1 + 8 * NR_neighbor];
    //Data event [NR_cond, x,y,z,value, x,y,z,value, ...]
    for (int s_no = 0; s_no < N_sample; s_no++)
    {
        int fid = ids[s_no + start_id];    //the real id in the sample
        vector < vector < int >>indices;
        getNearest_point(R, sample[fid].x, sample[fid].y, sample[fid].z,
                         NR_neighbor, indices);
        size_t n_neigh = indices[0].size();
        if (n_neigh > NR_neighbor)
            dv[0] = NR_neighbor;
        else
            dv[0] = n_neigh;
        if ((s_no + start_id) < 180)
            dv[0] = 0;
        dv_bnd bnd;
        memset(&bnd, 0, sizeof(int) * 6);
        int dv_seq_no = 0;
        for (int i = 0; i < dv[0]; i++)
        {
            int nfid = indices[0][i] - 1;
            nfid = ids[nfid]; 
            if (nfid == fid)
                continue;
            //construction of data event
            dv[1 + dv_seq_no * 4] =
                int ((sample[nfid].x - sample[fid].x) / unitsize);
            dv[1 + dv_seq_no * 4 + 1] =
                int ((sample[nfid].y - sample[fid].y) / unitsize);
            dv[1 + dv_seq_no * 4 + 2] =
                int ((sample[nfid].z - sample[fid].z) / unitsize);
            dv[1 + dv_seq_no * 4 + 3] = target[int
                                                (sample[nfid].x +
                                                 length * sample[nfid].y +
                                                 length * width *
                                                 sample[nfid].z)];
            if (dv[1 + 4 * dv_seq_no] < bnd.dvminx)
                bnd.dvminx = dv[1 + 4 * dv_seq_no];
            if (dv[1 + 4 * dv_seq_no] > bnd.dvmaxx)
                bnd.dvmaxx = dv[1 + 4 * dv_seq_no];
            if (dv[1 + 4 * dv_seq_no + 1] < bnd.dvminy)
                bnd.dvminy = dv[1 + 4 * dv_seq_no + 1];
            if (dv[1 + 4 * dv_seq_no + 1] > bnd.dvmaxy)
                bnd.dvmaxy = dv[1 + 4 * dv_seq_no + 1];
            if (dv[1 + 4 * dv_seq_no + 2] < bnd.dvminz)
                bnd.dvminz = dv[1 + 4 * dv_seq_no + 2];
            if (dv[1 + 4 * dv_seq_no + 2] > bnd.dvmaxz)
                bnd.dvmaxz = dv[1 + 4 * dv_seq_no + 2];
            dv_seq_no++;
            if (dv_seq_no > NR_neighbor)
                break;
        }
        if ((level == 0) && (s_no < 0))
            target[int
                    (sample[fid].x + length * sample[fid].y +
                     length * width * sample[fid].z)] = rand() % 50;
        else
            target[int
                    (sample[fid].x + length * sample[fid].y +
                     length * width * sample[fid].z)] =
                direct_sampling_cont(ti, tilength, tiwidth, tiheight, dv,
                                     NR_try, tolerance, bnd);
        addPoint(R, sample[fid].x, sample[fid].y, sample[fid].z);
    }
    delete[]dv;
}

void load_ti_3D(float **ti, char *file, int *length, int *width,
                int *height)
{
    string type;
    int tmp;
    fstream fs;
    fs.open(file);
    fs >> *length;
    fs >> *width;
    fs >> *height;
    fs >> tmp;
    fs >> type;
    int size = (*length) * (*width) * (*height);
    *ti = new float[size];
    for (int i = 0; i < size; i++)
    {
        fs >> (*ti)[i];
    }
    fs.close();
}
